/**
 * @file main.cpp
 * @author 黄梓峰 林奕涵 屠严之
 * @brief VEX队内赛
 * @version 1.0
 * @date 2022-11-19
 * 
 * @copyright Copyright (c) 2022
 * 
 * 仅以次代码纪念 2022.11.20 的小组队内赛冠军 【狗头】 
 */
#include "vex.h"
#include "RoboDrive.h"
#include "RoboDevice.h"
#include "RoboController.h"
#include "RoboDrive.h"
#include "RoboRoller.h"
#include "RoboClamp.h"
#include <cmath>

const double Pi = acos(-1);

using namespace vex;

void auto1();
void auto2();

competition Competition;

void pre_auton(void) {
    RoboDevice::RoboDeviceInit();
    // All activities that occur before the competition starts
    // Example: clearing encoders, setting servo positions, ...
}

/**
 * @brief 自动控制阶段
 * 
 */
void autonomous(void) {
    auto2();
}

void auto1(void){
    RoboDrive::Drive(0.6,2.5);
    wait(150, msec);
    RoboDrive::Drive(1.2, 0.8);
    wait(1150, msec);
    RoboDrive::Drive(0.5,2.5);
    wait(115, msec);
    RoboDrive::Drive(1.2, 0.8);
    wait(200, msec);
    RoboDrive::BrakeMotor();
    wait(150,msec);
    RoboClamp::RoboClampHead_Lock_1();
    wait(100,msec);
    RoboDrive::Drive(-1.2,-0.6);
    int pos = 0;
    RoboDevice::LeftBackMotor.resetPosition();
    while (pos>-185)
    {
        pos = RoboDevice::LeftBackMotor.position(deg);
        wait(10,msec);
    }
    RoboDrive::BrakeMotor();
    wait(150,msec);
    RoboDrive::Drive(-0.6,1.8);
    wait(350, msec);
    
    RoboClamp::RoboClampHead_Release_1();
    RoboDrive::Drive(-0.8,-0.7);
    wait(450,msec);
    RoboDrive::Rotate(-105,1);
    RoboDrive::Drive(0.6,2);
    wait(150, msec);
    RoboDrive::Drive(1.2, 0.8);
    wait(470, msec);
    RoboDrive::BrakeMotor();
    wait(200,msec);
    RoboClamp::RoboClampHead_Lock_1();
    wait(100,msec);
    RoboDrive::Drive(-0.6,-2.5);
    wait(200,msec);
    RoboDrive::Drive(-1.2, -0.8);
    wait(900, msec);
    RoboDrive::Rotate(-70,1.3);
    RoboDrive::Drive(-0.6,-2.5);
    wait(200,msec);
    RoboDrive::Drive(-1.2, -0.8);
    wait(1200, msec);
    
}

void auto2(){
     RoboDrive::MoveForward(1.4,1.1);
    wait(50,msec);
    RoboDrive::MovewithAngle(1.0,45,1);
    wait(100,msec);
    RoboClamp::RoboClampHead_Lock_1();//抓桶
    wait(50,msec);
    RoboDrive::Drive(-0.6,0);

    int pos = 0;
    RoboDevice::LeftBackMotor.resetPosition();
    while (pos>-185)
    {
        pos = RoboDevice::LeftBackMotor.position(deg);
        wait(10,msec);
    }//VERY GOOD
    wait(150,msec);
    RoboDrive::BrakeMotor();
    wait(150,msec);
    RoboDrive::Rotate(-78,1.5);
    RoboDrive::MoveBackward(1,0.8);
    RoboDrive::MovewithAngle(-0.8,100,1);
    RoboDrive::MoveBackward(1,1.3);
    // RoboDrive::MovewithAngle(-0.8,30,1);//回正
    // wait(100,msec);
    // RoboDrive::MovewithAngle(-1.4,100,1.5);
    // wait(100,msec);
    // RoboDrive::MoveForward(1.2,1.6);
    
}//目标是中桶, VERY GOOD

/**
 * @brief 手动控制阶段
 * 
 */
void usercontrol(void) {
    while (1) {
        RoboController::RoboMoveDetect(RoboDevice::Controller.Axis3,RoboDevice::Controller.Axis1);
        RoboController::RoboBrakeDetect(RoboDevice::Controller.ButtonB);                      //重设刹车按钮，初始为B
        RoboController::RoboturnlDetect(RoboDevice::Controller.ButtonL2);                     //重设左转90度按钮，初始为L1
        RoboController::RoboturnrDetect(RoboDevice::Controller.ButtonR2);                     //重设右转90度按钮，初始为R1
        RoboController::RoboRollerRollingDetectLeft(RoboDevice::Controller.ButtonLeft);       //默认滚筒左键方向为负
        RoboController::RoboRollerRollingDetectRight(RoboDevice::Controller.ButtonRight);     //默认滚筒右键方向为正
        RoboController::RoboClampHeadDetect(RoboDevice::Controller.ButtonR1);
        RoboController::RoboClampBackDetect(RoboDevice::Controller.ButtonL1);
        wait(20, msec); // Sleep the task for a short amount of time to prevent wasted resources.
    }
}

int main() {
    // Competition.autonomous(autonomous);
    // Competition.drivercontrol(usercontrol);

    // pre_auton();
    // // auto1();
    // // usercontrol();
    // // autonomous();

    // while (true) {
    //     wait(100, msec);
    // }
    auto2();
}
